
#include <stdio.h>

#include <BCapsule>
#include <BCube>
#include <BCone>
#include <BSphere>

#include "IBroadphase.h"
#include "ITriangleShape.h"
#include "IOverlapPair.h"
#include "IAlgorithm.h"
#include "IGjkEpaSolver.h"
#include "IConvexConvexAlgorithm.h"
#include "IContinuousConvexCast.h"
#include "IGjkPairDetector.h"
#include "member_BShape.h"
#include "member_BNode.h"
#include "member_BBody.h"

using namespace BWE;

#ifdef REAL_IS_DOUBLE
#define REAL_ERROR	1.0e-12
#else
#define REL_ERROR 1.0e-6f
#endif

IConvexConvexAlgorithm::IConvexConvexAlgorithm()
{

}
IConvexConvexAlgorithm::~IConvexConvexAlgorithm()
{

}

void IConvexConvexAlgorithm::caculate()
{
	_marginA = _shapeA->margin();
	_marginB = _shapeB->margin();

	if (dynamic_cast<BCapsule*>(_shapeA) && dynamic_cast<BCapsule*>(_shapeB))
	{
		BCapsule* capsuleA = (BCapsule*)_shapeA;
		BCapsule* capsuleB = (BCapsule*)_shapeB;
		BVector normalB;
		BVector pointB;
		BReal distance = capsuleCapsuleDistance(normalB, pointB, capsuleA->halfHeight(), capsuleA->radius(), capsuleB->halfHeight(), capsuleB->radius());
		if (distance < REAL_EPSILON)
		{
			this->addContactPoint(pointB, normalB, distance);
		}
		this->refreshContactPoints();
		return;
	}

	if (dynamic_cast<BCapsule*>(_shapeA) && dynamic_cast<BSphere*>(_shapeB))
	{
		BCapsule* capsule = (BCapsule*)_shapeA;
		BSphere* sphere = (BSphere*)_shapeB;
		BVector normalB;
		BVector pointB;
		BReal distance = capsuleCapsuleDistance(normalB, pointB, capsule->halfHeight(), capsule->radius(), 0., sphere->radius());
		if (distance < REAL_EPSILON)
		{
			this->addContactPoint(pointB, normalB, distance);
		}
		this->refreshContactPoints();
		return;
	}

	if (dynamic_cast<BSphere*>(_shapeA) && dynamic_cast<BCapsule*>(_shapeB))
	{
		BSphere* sphere = (BSphere*)_shapeA;
		BCapsule* capsule = (BCapsule*)_shapeB;
		BVector normalB;
		BVector pointOnB;
		BReal distance = capsuleCapsuleDistance(normalB, pointOnB, 0, sphere->radius(), capsule->halfHeight(), capsule->radius());
		if (distance < REAL_EPSILON)
		{
			this->addContactPoint(pointOnB, normalB, distance);
		}
		this->refreshContactPoints();
		return;
	}

	IDetectorResult result;
	IVoronoiSimplexSolver simplexSolver;
	IGjkPairDetector gjkPairDetector(_shapeA, _shapeB, &simplexSolver);
	gjkPairDetector.closestPoints(_matrixA, _matrixB, result);
	if (result.hasResult)
	{
		this->addContactPoint(result.pointB, result.normalB, result.distance);
	}
	this->refreshContactPoints();
}
void IConvexConvexAlgorithm::timeImpact()
{
	const BMatrix& matrixA = node_member(_bodyA)->matrix;
	const BMatrix& matrixB = node_member(_bodyB)->matrix;

	const BMatrix& interpMatrixA = body_member(_bodyA)->predictedMatrix;
	const BMatrix& interpMatrixB = body_member(_bodyB)->predictedMatrix;

	CastResult caseResult;
	IContinuousConvexCast ccc(_shapeA, _shapeB);
	ccc.calcTimeOfImpact(matrixA, interpMatrixA, matrixB, interpMatrixB, caseResult);
}

BVector IConvexConvexAlgorithm::segmentsClosestPoints(const BVector& offset, const BVector& directA, BReal heightA, const BVector& directB, BReal heightB, BVector& offsetA, BVector& offsetB)
{
	BReal dirA_dot_dirB = directA.dot(directB);
	BReal dirA_dot_trans = directA.dot(offset);
	BReal dirB_dot_trans = directB.dot(offset);
	BReal tA = 0;
	BReal tB = 0;

	BReal denom = 1 - dirA_dot_dirB * dirA_dot_dirB;
	if (denom > 0)
		tA = (dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB) / denom;
	tA = bClamp(tA, -heightA, heightA);

	tB = tA * dirA_dot_dirB - dirB_dot_trans;
	if (tB < -heightB)
	{
		tB = -heightB;
		tA = tB * dirA_dot_dirB + dirA_dot_trans;
	}
	else if (tB > heightB)
	{
		tB = heightB;
		tA = tB * dirA_dot_dirB + dirA_dot_trans;
	}
	tA = bClamp(tA, -heightA, heightA);

	offsetA = directA * tA;
	offsetB = directB * tB;
	return offset - offsetA + offsetB;
}
BReal IConvexConvexAlgorithm::capsuleCapsuleDistance(BVector& normalOnB, BVector& pointOnB, BReal heightA, BReal radiusA, BReal heightB, BReal radiusB)
{
	BVector directA = _matrixA.row(2);
	BVector positionA = _matrixA.position();
	BVector directB = _matrixB.row(2);
	BVector positionB = _matrixB.position();
	BVector offset = positionB - positionA;

	BVector offsetA;
	BVector offsetB;
	BVector ptsVector = segmentsClosestPoints(offset, directA, heightA, directB, heightB, offsetA, offsetB);

	BReal distance = ptsVector.length() - radiusA - radiusB;
	if (distance > REAL_EPSILON)
		return distance;

	BReal len2 = ptsVector.length2();
	if (len2 <= REAL_EPSILON)
	{
		BVector q;
		PlaneSpace(directA, normalOnB, q);
	}
	else
	{
		normalOnB = -ptsVector / sqrt(len2);
	}

	pointOnB = positionB + offsetB + normalOnB * radiusB;
	return distance;
}
